package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.smartdashboard.SendablePIDController;

public class TorqueController {
    // All of these are scaled for one volt.
    private double m_freeSpeed;
    private double m_freeCurrent;
    private double m_stallTorque;
    private double m_stallCurrent;
    private PIDSource m_source;
    private PIDOutput m_output;

    public TorqueController(PIDSource source, PIDOutput output,
            double freeSpeed, double freeCurrent, double stallTorque, double stallCurrent, double standardVoltage) {
        m_source = source;
        m_output = output;
        m_freeSpeed = freeSpeed / standardVoltage;
        m_freeCurrent = freeCurrent / standardVoltage;
        m_stallTorque = stallTorque / standardVoltage;
        m_stallCurrent = stallCurrent / standardVoltage;
    }

    public TorqueController(double p, double i, double d, PIDSource source, PIDOutput output, double period,
            double freeSpeed, double freeCurrent, double stallTorque, double stallCurrent, double standardVoltage) {
        m_freeSpeed = freeSpeed / standardVoltage;
        m_freeCurrent = freeCurrent / standardVoltage;
        m_stallTorque = stallTorque / standardVoltage;
        m_stallCurrent = stallCurrent / standardVoltage;
    }

    /**
     * @return the m_freeSpeed
     */
    public double getFreeSpeed() {
        return m_freeSpeed;
    }

    /**
     * @param m_freeSpeed the m_freeSpeed to set
     */
    public void setFreeSpeed(double m_freeSpeed) {
        this.m_freeSpeed = m_freeSpeed;
    }

    /**
     * @return the m_freeCurrent
     */
    public double getFreeCurrent() {
        return m_freeCurrent;
    }

    /**
     * @param m_freeCurrent the m_freeCurrent to set
     */
    public void setFreeCurrent(double m_freeCurrent) {
        this.m_freeCurrent = m_freeCurrent;
    }

    /**
     * @return the m_stallTorque
     */
    public double getStallTorque() {
        return m_stallTorque;
    }

    /**
     * @param m_stallTorque the m_stallTorque to set
     */
    public void setStallTorque(double m_stallTorque) {
        this.m_stallTorque = m_stallTorque;
    }

    /**
     * @return the m_stallCurrent
     */
    public double getStallCurrent() {
        return m_stallCurrent;
    }

    /**
     * @param m_stallCurrent the m_stallCurrent to set
     */
    public void setStallCurrent(double m_stallCurrent) {
        this.m_stallCurrent = m_stallCurrent;
    }
}